#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
方诺双臂控制脚本
提供简单的命令行接口，用于控制双臂的基本动作
"""

import sys
import argparse
import time
import os
import numpy as np

# 添加父目录到路径，以便导入teleop模块
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from teleop.robot_control.fangnuo_car import FangnuoCar

def go_home(use_real_arms=True):
    """
    将双臂移动到初始位置
    
    参数:
    use_real_arms: 是否使用真实机械臂
    """
    print("初始化FangnuoCar对象...")
    car = FangnuoCar(use_real_arms=use_real_arms)
    
    print("\n执行: 重置双臂到初始位置")
    car.go_home()
    
    # 等待机械臂到达初始位置
    print("等待机械臂到达初始位置...")
    time.sleep(5)
    
    # 获取当前状态
    status = car.status()
    print("\n当前双臂状态:")
    print(f"左臂位置: {status['left_arm']['position']}")
    print(f"右臂位置: {status['right_arm']['position']}")
    
    print("\n双臂已重置到初始位置")

def stop_arms(use_real_arms=True):
    """
    停止双臂的运动
    
    参数:
    use_real_arms: 是否使用真实机械臂
    """
    print("初始化FangnuoCar对象...")
    car = FangnuoCar(use_real_arms=use_real_arms)
    
    print("\n执行: 停止双臂运动")
    car.stop()
    
    # 等待停止命令生效
    time.sleep(1)
    
    # 获取当前状态
    status = car.status()
    print("\n当前双臂状态:")
    print(f"左臂位置: {status['left_arm']['position']}")
    print(f"右臂位置: {status['right_arm']['position']}")
    
    print("\n双臂已停止")

def reset_arms(use_real_arms=True):
    """
    重置双臂（使用reset方法）
    
    参数:
    use_real_arms: 是否使用真实机械臂
    """
    print("初始化FangnuoCar对象...")
    car = FangnuoCar(use_real_arms=use_real_arms)
    
    print("\n执行: 重置双臂")
    car.reset()
    
    # 等待重置命令生效
    time.sleep(3)
    
    # 获取当前状态
    status = car.status()
    print("\n当前双臂状态:")
    print(f"左臂位置: {status['left_arm']['position']}")
    print(f"右臂位置: {status['right_arm']['position']}")
    
    print("\n双臂已重置")

def main():
    """主函数，解析命令行参数并执行相应的操作"""
    parser = argparse.ArgumentParser(description="方诺双臂控制脚本")
    parser.add_argument('action',default='home', choices=['home', 'stop', 'reset'], 
                        help="要执行的动作: home(回到初始位置), stop(停止运动), reset(重置机械臂)")
    parser.add_argument('--sim', action='store_true', 
                        help="使用模拟机械臂而不是真实机械臂")
    
    args = parser.parse_args()
    
    # 根据命令行参数决定是否使用真实机械臂
    use_real_arms = not args.sim
    
    # 执行相应的动作
    if args.action == 'home':
        go_home(use_real_arms)
    elif args.action == 'stop':
        stop_arms(use_real_arms)
    elif args.action == 'reset':
        reset_arms(use_real_arms)

if __name__ == "__main__":
    main()
